#include "main.h"


//delta L = kcl * delta C
// L :mm
//float dl,kcl=1500.0/21557.5; // :mm/c
//float klc = 21557.5/1500.0; //  :c/mm

//float kcl = 2*pi*32.5/(1200.0*39.0); //mm/c
//float klc = 1200.0*39.0/(2*pi*32.5); //c/mm
#define pi 3.1415926535898

float kcl = 70.0 * pi / 1716.0;   //mm/c, that may comes from that, 1:33 rounds, and 13 pulses per round
float klc = 1716.0 / (70.0 * pi); //c/mm 1560


int c1, c2;
char encoder_bufs[256];
char encoder_bufs_rear = 0, encoder_bufs_font = 0;


// c4: A1
// c5: B1
// c6: A2
// c7: B2
int encoder_do_count()
{
    static char A1 = 0, B1 = 0, A2 = 0, B2 = 0;
    static char M1 = 0, M2 = 0;
    const char motor1 = encoder_a1 | encoder_b1;
    const char motor2 = encoder_a2 | encoder_b2;
    // a1 = p & encoder_a;
    // b1 = p & encoder_b;
    // if (a1 != A1)
    // {
    //     c2 += (a1 << 1) == b1 ? 1 : -1;
    //     A1 = a1;
    // }
    // if (b1 != B1)
    // {
    //     c2 += (a1 << 1) == b1 ? -1 : +1;
    //     B1 = b1;
    // }

    while (encoder_bufs_rear != encoder_bufs_font)
    {
        char p = encoder_bufs[encoder_bufs_font++];
        char m1 = p & motor1;
        char m2 = p & motor2;
        if (m1 != M1)
        {
            char a1 = p & encoder_a1;
            char b1 = p & encoder_b1;
            if (a1 != A1)
            {
                c2 += (a1 << 1) == b1 ? -1 : 1;
                A1 = a1;
            }
            if (b1 != B1)
            {
                c2 += (a1 << 1) == b1 ? 1 : -1;
                B1 = b1;
            }
            M1 = m1;
        }
        else if (m2 != M2)
        {
            char a2 = p & encoder_a2;
            char b2 = p & encoder_b2;
            if (a2 != A2)
            {
                c1 += (a2 << 1) == b2 ? -1 : 1;
                A2 = a2;
            }
            if (b2 != B2)
            {
                c1 += (a2 << 1) == b2 ? 1 : -1;
                B2 = b2;
            }
            M2 = m2;
        }
    }

    return 0;
}
void encoder_int()
{
    const int ps = encoder_a1 | encoder_b1 | encoder_a2 | encoder_b2;

    GPIOIntClear(encoder_port, ps);
    int p = GPIOPinRead(encoder_port, ps);
    encoder_bufs[encoder_bufs_rear++] = p;
}

void init_encoder_int()
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
    //GPIO注册中断
    // GPIOIntRegister(GPIO_PORTF_BASE, PortFIntHandler);
    GPIOIntRegister(encoder_port, encoder_int);
    int ps = encoder_a1 | encoder_b1 | encoder_a2 | encoder_b2;
    //PF4作为中断输入源对应LaunchPad的按键1 SW1  PA2 PA3 作为编码器输入AB
    GPIOPinTypeGPIOInput(encoder_port, ps);

    //配置PA2 PA3为上拉电阻，输出电流能力2mA
    GPIOPadConfigSet(encoder_port, ps, GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU); //
    //终端类型为双边沿触发
    GPIOIntTypeSet(encoder_port, ps, GPIO_BOTH_EDGES);
    //使能PA2,PA3中断
    GPIOIntEnable(encoder_port, ps);
    //全局中断
    IntMasterEnable();
}
